#ifndef _ATOMIC_0701_DDZT_H_
#define _ATOMIC_0701_DDZT_H_

/*------------------------ Include Begin --------------------------*/

// C
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <string>
#include <math.h>
#include <time.h>
#include <ctype.h>

// C++
#include <iostream>
#include <fstream>
#include <chrono>
#include <string>
#include <random>
#include <cmath>
#include <vector>
#include <map>
#include <set>
#include <stdbool.h>
#include <sstream>

/*------------------------  Include End  -------------------------*/

/*------------------------  Service Info Begin  --------------------------*/
/**
 *
 * @name:     atomic_0701_ddzt
 * @chinese_name: 导弹状态
 * @description: information atomic_0701_ddzt
 * @version: V1.0.0
 * @build_time: 2024-05-23 16:00:00
 * @license: MIT
 * @service_path: atomic_0701_ddzt
 *
 * @resource_requirement:
 *         + cpu_architecture: x86_64
 *         + memory: 4GB
 *         + hard_disk: 50GB
 *
 * @developer:
 *      +name: Spiderman
 *      +email: Spidermsan@atomic_0701_ddzt.com
 * @maintainer:
 *      +name: yishui
 *      +email:yishui@atomic_0701_ddzt.com
 * @operating_system:
 *      +name:Linux
 *      +v1:5.13.0-117-generic
 *      +v2:5.14.0-117-generic
 *      +name:SylixOS
 *      +v1:1.0.0
 *      +v2:1.0.2
 *
 *
 *
 */
/*------------------------  Service Info End  --------------------------*/

/*------------------------  Return Type Begin  --------------------------*/

#define ATOMIC_SERVICE_SUCCESS 0
#define ATOMIC_SERVICE_FAIL -1
#define ATOMIC_SERVICE_TIMEOUT -2
#define ATOMIC_SERVICE_ERROR -3
#define ATOMIC_SERVICE_NOT_SUPPORT -4
#define ATOMIC_SERVICE_NOT_READY -5
#define ATOMIC_SERVICE_NOT_INIT -6
#define ATOMIC_SERVICE_NOT_START -7
#define ATOMIC_SERVICE_NOT_STOP -8
#define ATOMIC_SERVICE_NOT_RELEASE -9
#define ATOMIC_SERVICE_NOT_DESTROY -10
#define ATOMIC_SERVICE_NOT_CREATE -11
#define ATOMIC_SERVICE_NOT_OPEN -12
#define ATOMIC_SERVICE_NOT_CLOSE -13
#define ATOMIC_SERVICE_NOT_READ -14
#define ATOMIC_SERVICE_NOT_WRITE -15
#define ATOMIC_SERVICE_NOT_SEEK -16
#define ATOMIC_SERVICE_NOT_TELL -17
#define ATOMIC_SERVICE_NOT_SIZE -18
#define ATOMIC_SERVICE_NOT_SEEK_END -19
#define ATOMIC_SERVICE_NOT_SEEK_SET -20
#define ATOMIC_SERVICE_NOT_SEEK_CUR -21
#define ATOMIC_SERVICE_NOT_SEEK_ABS -22

/*------------------------  Return Type End  --------------------------*/

#define PI 3.14159265358979323846

/*------------------------  Message Info Begin  --------------------------*/

// 导弹状态输入参数 (基本数据类型)
typedef struct {
    double velocity;      // 当前速度(m/s)
    double altitude;      // 当前高度(m)
    double angle;         // 当前角度(度)
    double acceleration_x; // X轴加速度(m/s²)
    double acceleration_y; // Y轴加速度(m/s²)
    double acceleration_z; // Z轴加速度(m/s²)
    double gps_latitude;   // GPS纬度
    double gps_longitude;  // GPS经度
    double timestamp;      // 时间戳(s)
} MissileStateInput;

// 导弹状态输出参数 (基本数据类型)
typedef struct {
    double missile_velocity;  // 计算后的速度(m/s)
    double missile_altitude;  // 计算后的高度(m)
    double missile_angle;     // 计算后的角度(度)
    double yaw_rate;             // 偏航角速度(度/s)
    double pitch_rate;           // 俯仰角速度(度/s)
    double roll_rate;            // 滚转角速度(度/s)
    double filtered_acceleration_x; // 滤波后X轴加速度(m/s²)
    double filtered_acceleration_y; // 滤波后Y轴加速度(m/s²)
    double filtered_acceleration_z; // 滤波后Z轴加速度(m/s²)
    double estimated_latitude;    // 估计纬度
    double estimated_longitude;   // 估计经度
    double velocity_error;        // 速度误差
    double altitude_error;        // 高度误差
    double angle_error;           // 角度误差
    double kalman_gain_velocity;  // 速度卡尔曼增益
    double kalman_gain_altitude;  // 高度卡尔曼增益
    double kalman_gain_angle;     // 角度卡尔曼增益
    double covariance_velocity;  // 速度协方差
    double covariance_altitude;  // 高度协方差
    double covariance_angle;      // 角度协方差
} MissileStateOutput;


/*------------------------  Message Info End  --------------------------*/

// 扩展卡尔曼滤波器状态
typedef struct {
    double x[6];    // 状态向量: [位置x, 位置y, 位置z, 速度x, 速度y, 速度z]
    double P[6][6]; // 协方差矩阵
    double Q[6][6]; // 过程噪声协方差
    double R[3][3]; // 测量噪声协方差
} EKFState;


/*------------------------  Interface Declaration Begin  --------------------------*/

int getMissileStateData(MissileStateInput *input, MissileStateOutput *output);

/*------------------------  Interface Declaration End  --------------------------*/

// 初始化扩展卡尔曼滤波器
void initEKF(EKFState *ekf);
// 扩展卡尔曼滤波器预测步骤
void predictEKF(EKFState *ekf, double dt, double ax, double ay, double az);
// 扩展卡尔曼滤波器更新步骤
void updateEKF(EKFState *ekf, double zx, double zy, double zz);




#endif // _ATOMIC_0701_DDZT_H_
